#include "msg_convert.h"

namespace msg_convert{

    // tf2::Quaternion -> Euler
    std::tuple<double, double, double> Quaternion2Euler(tf2::Quaternion quaternion){
        tf2::Matrix3x3 mat(quaternion);
        double roll, pitch, yaw;
        mat.getRPY(roll, pitch, yaw);
        return std::make_tuple(roll, pitch, yaw);
    }

    // geometry_msgs::Quaternion -> Euler
    std::tuple<double, double, double> GeometryQuaternion2Euler(geometry_msgs::Quaternion orientation){
        tf2::Quaternion quaternion;
        tf2::fromMsg(orientation, quaternion);
        return Quaternion2Euler(quaternion);
    }

    // Euler -> tf2::Quaternion
    tf2::Quaternion Euler2Quaternion(double roll, double pitch, double yaw){
        tf2::Quaternion quaternion;
        quaternion.setRPY(roll, pitch, yaw);
        return quaternion;
    }

    // Euler -> tf2::Quaternion
    geometry_msgs::Quaternion Euler2GeometryQuaternion(double roll, double pitch, double yaw){
        geometry_msgs::Quaternion orientation;
        tf2::Quaternion quaternion = Euler2Quaternion(roll, pitch, yaw);
        orientation = tf2::toMsg(quaternion);
        return orientation;
    }

    // Pose2D -> geometry_msgs::Pose
    geometry_msgs::Pose Pose2D2Pose(Pose2D pose_2d){
        geometry_msgs::Pose pose;
        // pose.position.x = tf2::getYaw(msg->pose.orientation);
        pose.position.x = pose_2d.x;
        pose.position.y = pose_2d.y;
        pose.orientation = Euler2GeometryQuaternion(0.0, 0.0, pose_2d.yaw);
        return pose;
    }

    // geometry_msgs::Pose -> Pose2D
    Pose2D Pose2Pose2D(geometry_msgs::Pose pose){
        Pose2D pose_2d;
        pose_2d.x = pose.position.x;
        pose_2d.y = pose.position.y;
        pose_2d.yaw = tf2::getYaw(pose.orientation);
        return pose_2d;
    }

    // Pose2D -> geometry_msgs::TransformStamped
    geometry_msgs::TransformStamped Pose2D2TransformStamped(Pose2D pose_2d){
        geometry_msgs::TransformStamped transform;
        transform.transform.translation.x = pose_2d.x;
        transform.transform.translation.y = pose_2d.y;
        transform.transform.rotation = Euler2GeometryQuaternion(0.0, 0.0, pose_2d.yaw);
        return transform;
    }

    // tf2::Transform -> geometry_msgs::TransformStamped
    geometry_msgs::TransformStamped Transform2Geometry(tf2::Transform transform_tf){
        geometry_msgs::TransformStamped transform_stamped;
        transform_stamped.transform = tf2::toMsg(transform_tf);
        return transform_stamped;
    }

    // geometry_msgs::TransformStamped -> tf2::Transform
    tf2::Transform Geometry2Transform(geometry_msgs::TransformStamped transform_stamped){
        tf2::Transform transform_tf;
        tf2::fromMsg(transform_stamped.transform, transform_tf);
        return transform_tf;  
    }



} //namespace MapConvert